#include "pid.h"
#include <chrono>
#include <time.h>
#include <sys/time.h>

const double PID_PI =  3.141592653589793f;



#if 1 
//cpp function
 static int64_t getTimeMillis()
{
	int64_t timeMs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

	return timeMs;
}
#else 
//c function
static long getTimeUsec()
{
    struct timeval t;
    gettimeofday(&t, 0);
    return (long)((long)t.tv_sec * 1000 * 1000 + t.tv_usec);
}

static long getTimeMillis()
{
    return getTimeUsec() / 1000;
}
#endif



float PID::getPid(float error, float scaler)
{
    int64_t tnow = getTimeMillis();
    int64_t dt = tnow - m_lastTime;
    float output            = 0;
    float deltaTime;

    if (m_lastTime == 0 || dt > 1000)
	{
        dt = 0;

		/* 
		** if this PID hasn't been used for a full second then zero
		** the intergator term. This prevents I buildup from a
		** previous fight mode from causing a massive return before
		** the integrator gets a chance to correct itself
		*/
		resetI();
    }
    m_lastTime = tnow;

    deltaTime = (float)dt / 1000.0f;

    // Compute proportional component
    m_pidInfo.P = error * m_kp;
    output += m_pidInfo.P;

    // Compute derivative component if time has elapsed
    if ((fabsf(m_kd) > 0) && (dt > 0))
	{
        float derivative;

		if (std::isnan(m_lastDerivative))
		{
			// we've just done a reset, suppress the first derivative
			// term as we don't want a sudden change in input to cause
			// a large D output change			
			derivative = 0;
			m_lastDerivative = 0;
		}
		else
		{
			derivative = (error - m_lastError) / deltaTime;
		}

        // discrete low pass filter, cuts out the
        // high frequency noise that can drive the controller crazy
        float RC = 1/(2*PID_PI*m_fCut);
        derivative = m_lastDerivative +
                     ((deltaTime / (RC + deltaTime)) *
                      (derivative - m_lastDerivative));

        // update state
        m_lastError             = error;
        m_lastDerivative    = derivative;

        // add in derivative component
        m_pidInfo.D = m_kd * derivative;
        output                          += m_pidInfo.D;
    }

    // scale the P and D components
    output *= scaler;
    m_pidInfo.D *= scaler;
    m_pidInfo.P *= scaler;

    // Compute integral component if time has elapsed
    if ((fabsf(m_ki) > 0) && (dt > 0))
	{
        m_integrator             += (error * m_ki) * scaler * deltaTime;
        if (m_integrator < -m_imax)
		{
            m_integrator = -m_imax;
        }
		else if (m_integrator > m_imax)
		{
            m_integrator = m_imax;
        }
        m_pidInfo.I = m_integrator;
        output                          += m_integrator;
    }

    m_pidInfo.desired = output;
    return output;
}

void
PID::resetI()
{
    m_integrator = 0;
	// we use NAN (Not A Number) to indicate that the last 
	// derivative value is not valid
    m_lastDerivative = NAN;
}
